/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.RateGyro;
import com.sun.squawk.util.MathUtils;

/**
 *
 * @author matt
 */
public class InertialReference {

    private final static InertialReference IntertialReferneceSingleton = new InertialReference();

    public static InertialReference getInstance() {
        return IntertialReferneceSingleton;
    }

    public class GravityVector {

        double x;
        double y;
        double z;

        public GravityVector(double x_in, double y_in, double z_in) {
            x = x_in;
            y = y_in;
            z = z_in;
        }

        public GravityVector(GravityVector v_in) {
            x = v_in.x;
            y = v_in.y;
            z = v_in.z;
        }

        public double includedAngle(GravityVector v_new) {
            // return the included angle between the internal vector and the
            //  v_new vector
            
            // Normalize the internal vector v
            //length_i = Math.sqrt(x*x+y*y+z*z)
            //norm_x = x/length_i
            //norm_y = y/length_i
            //norm_z = z/length_i
            //
            //// Normalize the new vector nv
            //length_n = Math.sqrt(v_new.x*v_new.x + v_new.y*v_new.y + v_new.z*v_new.z)
            //norm_nx = v_new.x/lenhth_n
            //norm_ny = v_new.y/lenhth_n
            //norm_nx = v_new.z/lenhth_n
            //
            //// take the dot product v dot nv
            //v_dot_nv = norm_x*norm_nx +norm_y*norm_ny +norm_z*norm_nz;
            //
            //// and then then angle is acos(v dot nv) converted to degrees
            //included = MathUtils.acos(v_dot_nv) * (180/Math.PI);
            //return included;
            return 0;
        }

        public double tiltHeading(GravityVector v_new) {
            //return the heading of the line from the internal vector to the
            // v_new vector projected onto the XY plane
            //simple projection by deleteing z
            // NEED to deal with cases where atan2 fails...
            //return MathUtils.atan2(v_new.x-x, v_new.y-y) * (180.Math.PI);
            return 0;
        }
    }
    // Guessing that 4G is enough resolution
    // 3 Axis Accelerometer
    private ADXL345_I2C m_Accelerometer = new ADXL345_I2C(RobotMap.kAccelerometerDigitalPort, ADXL345_I2C.DataFormat_Range.k4G);
    // Yaw Gyro (Rotation about Z)
    private RateGyro m_YawGyro = new RateGyro(RobotMap.kYawGyroPort);
    // Pitch Gyro (Rotation about Y (L-R axis)
    private RateGyro m_PitchGyro = new RateGyro(RobotMap.kPitchGyroPort);
    private double m_pitchOffset = 0;
    private double m_rollOffset = 0;

    private InertialReference() {
        m_YawGyro.reset();
        m_PitchGyro.reset();

        // compute angular offsets for accelerometer axes
        // assumes constructor is run while robot is stationary and level
        m_pitchOffset = getAPitch();
        m_rollOffset = getARoll();
        System.out.println("InertialReference Constructed");
    }

    public GravityVector getGravityVector() {
        return new GravityVector(getRawX(), getRawY(), getRawZ());
    }

    public double getAPitch() {
        // compute pitch based on accelerometer and 1g down
        // pitch = -acos(Zs/||Zs+Ys||)
        // pitch = -asin(Ys/||Zs+Ys||)

        double gY = getRawY();
        double gZ = getRawZ();

        double hyp = Math.sqrt((gZ * gZ + gY * gY));

        double pitch_y = -MathUtils.acos(gY / hyp);
        double pitch_z = -MathUtils.asin(gY / hyp);


        if (Math.abs(pitch_y - pitch_z) > 0.001) {
            if (gY > gZ) {
                return pitch_y * (180/Math.PI) - m_pitchOffset;
            } else {
                return pitch_z * (180/Math.PI) - m_pitchOffset;
            }
        } else {
            return pitch_z * (180/Math.PI) - m_pitchOffset;
        }
    }

    public double getARoll() {
        // compute Roll based on accelerometer and 1g down
        // roll = acos(Zs/||Zs+Xs||)
        // roll = asin(Xs/||Zs+Xs||)
        // compute pitch based on accelerometer and 1g down
        // pitch = -acos(Zs/||Zs+Ys||)
        // pitch = -asin(Ys/||Zs+Ys||)

        double gX = getRawX();
        double gZ = getRawZ();

        double hyp = Math.sqrt((gZ * gZ + gX * gX));

        double pitch_y = -MathUtils.acos(gX / hyp);
        double pitch_z = -MathUtils.asin(gX / hyp);


        if (Math.abs(pitch_y - pitch_z) > 0.001) {
            if (gX > gZ) {
                return pitch_y * (180/Math.PI) - m_rollOffset;
            } else {
                return pitch_z * (180/Math.PI) - m_rollOffset;
            }
        } else {
            return pitch_z * (180/Math.PI) - m_rollOffset;
        }
    }

    public double getRawX() {
        double accel = m_Accelerometer.getAcceleration(ADXL345_I2C.Axes.kX);
        return accel;
    }

    public double getRawY() {
        double accel = m_Accelerometer.getAcceleration(ADXL345_I2C.Axes.kY);
        return accel;
    }

    public double getRawZ() {
        double accel = m_Accelerometer.getAcceleration(ADXL345_I2C.Axes.kZ);
        return accel;
    }

    public double getRawYaw() {
        double yaw = m_YawGyro.getAngle();
        return yaw;
    }

    public double getRawYawRate() {
        double yawRate = m_YawGyro.getAngleRate();
        return yawRate;
    }

    public double getRawPitch() {
        double pitch = m_PitchGyro.getAngle();
        return pitch;
    }

    public double getRawPitchRate() {
        double pitchRate = m_PitchGyro.getAngleRate();
        return pitchRate;
    }
}
